#ifndef __UDSF_CFAR_C_
#define __UDSF_CFAR_C_

#include "udf_global.h"
/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "UDSF.c"形式引用
*/
#include "udsf_cstdlike.c"

// 最后修改：2023年6月5日 09点32分 V1.0
static void udsf_cfarframe(uint8_t chirp_dir,uint8_t data_inc,STRUCT_POINTRST *sRst,BBE_CFARRST_T *sCfar,uint32_t cfarNum,uint32_t *powAbs,_Bool *hitAFlag,uint8_t cfarFlag) __attribute__((unused));

// chirp dir: 0-row       data_inc = UDF_RANGE_USE 
//            1-column    data_inc = UDF_VEL_NUM
static void udsf_cfarframe(uint8_t chirp_dir,uint8_t data_inc,STRUCT_POINTRST *sRst,BBE_CFARRST_T *sCfar,uint32_t cfarNum,uint32_t *powAbs,_Bool *hitAFlag,uint8_t cfarFlag){
	
	sRst->targetNum=0;
	
	switch(cfarFlag){
		case CFAR1_EN: //cfar1 vel
			for (uint32_t targetNow=0;targetNow<cfarNum;targetNow++){
				if(sCfar[targetNow].interIdx<paramFunc->rangeIdxTh){
					
					sRst->target[sRst->targetNum].d1Idx = sCfar[targetNow].interIdx;	
					sRst->target[sRst->targetNum].d2Idx = sCfar[targetNow].intraIdx;
					sRst->target[sRst->targetNum].d3Idx = 0;
					
					if(  chirp_dir == 0 ){ // 行存储方式
						sRst->target[sRst->targetNum].powABS = powAbs[sCfar[targetNow].intraIdx*data_inc+sCfar[targetNow].interIdx];
					}else{                 // 列存储方式
						sRst->target[sRst->targetNum].powABS = powAbs[sCfar[targetNow].intraIdx + sCfar[targetNow].interIdx*data_inc];
					}
					sRst->targetNum++;	
				}	
			}			
			break;
		case CFAR2_EN: //cfar2 range
			for (uint32_t targetNow=0;targetNow<cfarNum;targetNow++){
				if(sCfar[targetNow].intraIdx<paramFunc->rangeIdxTh){
					
					sRst->target[sRst->targetNum].d1Idx = sCfar[targetNow].intraIdx;	
					sRst->target[sRst->targetNum].d2Idx = sCfar[targetNow].interIdx;	
					sRst->target[sRst->targetNum].d3Idx = 0;
					
					if( chirp_dir == 0 ){
						sRst->target[sRst->targetNum].powABS = powAbs[sCfar[targetNow].intraIdx + sCfar[targetNow].interIdx*data_inc];
					}else{
						sRst->target[sRst->targetNum].powABS = powAbs[sCfar[targetNow].intraIdx*data_inc + sCfar[targetNow].interIdx];
					}
					sRst->targetNum++;			
				}
			}
			break;
		case CFAR12_EN: //cfar1+cfar2  vel+range=vel
			for (uint32_t targetNow=0;targetNow<cfarNum;targetNow++){
				if(hitAFlag[targetNow]&&sCfar[targetNow].interIdx<paramFunc->rangeIdxTh){
					
					sRst->target[sRst->targetNum].d1Idx = sCfar[targetNow].interIdx;	
					sRst->target[sRst->targetNum].d2Idx = sCfar[targetNow].intraIdx;
					sRst->target[sRst->targetNum].d3Idx = 0;
					if( chirp_dir == 0 ){ // 行存储方式
						sRst->target[sRst->targetNum].powABS = powAbs[sCfar[targetNow].intraIdx*data_inc+sCfar[targetNow].interIdx];
					}else{                 // 列存储方式
						sRst->target[sRst->targetNum].powABS = powAbs[sCfar[targetNow].intraIdx + sCfar[targetNow].interIdx*data_inc];
					}
					sRst->targetNum++;						
				}
			}
			break;
	}
}
#if 0 // 待优化，去除全局变量使用 BBE_ABUF0_BASE
static int udsf_manual_osCfar(uint32_t startAddr, uint32_t useRange, uint32_t dstAddr,uint8_t range_thres, uint8_t thresFac) __attribute__((unused));
static int udsf_manual_osCfar(uint32_t startAddr, uint32_t useRange, uint32_t dstAddr, uint8_t range_thres,uint8_t thresFac)
{
    uint32_t headi, ret;
    ret = 0;
		//fft1d minus abs mean
		paramALG->alg9.p2Cfg0 = (paramALG->alg9.p2Cfg0&0x000Fffff) | ((useRange-1)<<20);
    paramALG->alg9.p2Cfg4 = (NUM_BYTE32) << 16 | (startAddr & 0xFFFF) << 0;
    paramALG->alg9.p2Cfg8 = (0) << 16 | (BBE_ABUF0_BASE & 0xFFFF) << 0;
    P2_FFT_HANDLE(&paramALG->alg9);
    //abs mean * 1.5
    uint32_t absThres = Read_M32(BBE_ABUF0_BASE) * 3;
    uint8_t delCnt = 0;
    for (headi = 0; headi < useRange; headi++)
    {
        if (Read_M32(startAddr + 0x10000 + headi * 4) > absThres)
        {
            Write_M32(dstAddr + headi * 4, 0);
            delCnt++;
        }
        else
        {
            Write_M32(dstAddr + headi * 4, Read_M32(startAddr + headi * 4));
        }
    }
    //fft1d minus abs mean after delete large value
    paramALG->alg9.p2Cfg0 = (paramALG->alg9.p2Cfg0&0x000Fffff) | ((useRange-1)<<20);
		paramALG->alg9.p2Cfg4 = (NUM_BYTE32) << 16 | ((dstAddr) & 0xFFFF) << 0;
    P2_FFT_HANDLE(&paramALG->alg9);
    // Set os cfar threshold
    absThres = Read_M32(BBE_ABUF0_BASE) * thresFac * 2;
    for (headi = 0; headi < range_thres; headi++)
    {
        if (Read_M32(startAddr + 0x10000 + headi * 4) > absThres)
        {
            ret = 1;
            break;
        }
    }
    return ret;
}


//#include "udsf_uart.c"
//#include "udsf_spi.c"

static void udsf_manual_osCfar2(uint8_t chirp_index, uint8_t useRange,uint32_t startAddr,uint32_t tmpAddr, BBE_CFARRST_T * cfarrst,uint8_t*target_cnt,uint8_t thresFac,uint8_t target_max) __attribute__((unused));

static void udsf_manual_osCfar2(uint8_t chirp_index, uint8_t useRange,uint32_t startAddr,uint32_t tmpAddr, BBE_CFARRST_T * cfarrst,uint8_t*target_cnt,uint8_t thresFac,uint8_t target_max)
{
	int target_valid_cnt = 0;
	
	if( *target_cnt == CFAR_MAX_NUM ){
		return;
	}

	udsf_memcpy((uint8_t*)startAddr,(uint8_t*)tmpAddr,useRange*4);

	
	P2_FFT_CONFIG *alg_cfg = &paramALG->alg26;
	
	uint32_t dst_addr = BBE_ABUF0_BASE;
	
	uint32_t recover = *((uint32_t*)dst_addr);

	//求均值
	alg_cfg->p2Cfg0 = (alg_cfg->p2Cfg0&0x000Fffff) | ((useRange-1)<<20);
	alg_cfg->p2Cfg4 = (NUM_BYTE32) << 16 | (GET_OFFSET(tmpAddr) & 0xFFFF) << 0;
	alg_cfg->p2Cfg8 = (0) << 16 | (GET_OFFSET(dst_addr) & 0xFFFF) << 0;
	P2_FFT_HANDLE(alg_cfg);
	
	//abs mean * 1.5
	uint32_t absThres = Read_M32(dst_addr) * 3;

	//LOG_SEND_STRING(1,&absThres,1,"\n\nabs mean*1.5 ","\n");
	
	uint8_t delCnt = 0;
	for (int i = 0; i < useRange; i++)
	{			
		uint32_t *pxx = ((uint32_t*)(tmpAddr + BBE_ABS_OFFSET + i * 4));
		
		if (*pxx > absThres)
		{
			*((uint32_t*)(tmpAddr + i * 4)) = 0;
			delCnt++;
		}
		
		//LOG_SEND_STRING(1,pxx,1,"pt ","\n");
	}
	uint8_t fac = useRange/(useRange-delCnt) + 2; 
	
	alg_cfg->p2Cfg0 = (alg_cfg->p2Cfg0&0x000Fffff) | ((useRange-1)<<20);
	alg_cfg->p2Cfg4 = (NUM_BYTE32) << 16 | (GET_OFFSET(tmpAddr) & 0xFFFF) << 0;
	P2_FFT_HANDLE(alg_cfg);
	
	udsf_memcpy((uint8_t*)startAddr,(uint8_t*)tmpAddr,useRange*4);

	
//	LOG_SEND_U8_BASE16(&delCnt,1,"delcnt ","\n");
//	LOG_SEND_U8_BASE16(&fac,1,"fac ","\n");
	
	// Set os cfar threshold
	absThres = Read_M32(dst_addr) * thresFac * fac;
	
//	LOG_SEND_STRING(1,&absThres,1,"abs thres ","\n");
	
	#if 1 // 取峰值
	#define FILTER_INTV      (3)  // 间隔达N个点，强制截断
	#define FILTER_FALL_CNT  (127)  // 连续N点下降，判定为峰值点
	#define FILTER_RESET_CNT (1) // 连续上升次数
	
	uint32_t max = 0;
	uint8_t max_index = 0;

	int fall_cnt=0;
	int valid_cnt = 0;
	
	int lock = 0;
	uint32_t last_value = 0;
	
	int cnt = 0;
	
	for (int i = 0; i < useRange; i++)
	{
		uint32_t *pxx = ((uint32_t*)(tmpAddr + BBE_ABS_OFFSET + i * 4));
		
		int valid_flag = 0;
		
		if( lock ){//&& (i+1 < useRange)){
			
			if( *pxx > last_value ){
				cnt ++;
				if( cnt >= FILTER_RESET_CNT ){ // 有N次非连续 增长趋势，解锁
					lock = 0;				
				}else{
					continue;
				}
			}else{
				last_value = *pxx;
				continue;
			}
		}
		cnt = 0;
		
		last_value = *pxx;
		if( *pxx > absThres ){
			last_value = 0;
			valid_cnt=FILTER_INTV;
			if( *pxx > max ){      // 记录最大值
				max = *pxx;
				max_index = i;
				fall_cnt = 0;
			}else{
				fall_cnt++;
				if( fall_cnt >= FILTER_FALL_CNT ){ // 连续两次未更新最大值，取最大值为峰值
					if( max > 0 ){     // 存在最大值则取为峰值
						valid_flag = 1;
						lock = 1; // 加锁
					}
				}
			}
		}else{  
			if( valid_cnt != 0 ){  // 连续N个bin无无效，取最大值为峰值
				valid_cnt --;
				if( valid_cnt == 0 ){
					if( max > 0 ){     // 存在最大值则取为峰值
						valid_flag = 1; 
					}
				}
			}
		}
		
		if( i == (useRange-1) ){ // 循坏结束 强制取上个有效点为峰值

			if( valid_cnt ){
				if( max > 0 ){
					valid_flag = 1;
				}
			}
		}
		
		if( valid_flag ){  // 记录峰值点
			
			cfarrst[*target_cnt].interIdx = chirp_index;
			cfarrst[*target_cnt].intraIdx = max_index;

			if( *target_cnt < CFAR_MAX_NUM ){
				*target_cnt += 1;
			}else{
				break;
			}
			
			target_valid_cnt ++ ;
			if( target_max <= target_valid_cnt ){
				break;
			}				
			max = 0;
			fall_cnt = 0;
		}
		#else
		if (*pxx > absThres)
		{			
			cfarrst[*target_cnt].interIdx = chirp_index;
			cfarrst[*target_cnt].intraIdx = i;

			if( *target_cnt < CFAR_MAX_NUM ){
				*target_cnt += 1;
			}
		}
		#endif
	}
	
	*((uint32_t*)dst_addr) = recover;
}


#endif
static int udsf_hw_cfar(uint8_t chirp_dir,STRUCT_POINTRST *frameRst,CFAR_CONFIG* range_cfar,CFAR_CONFIG* vel_cfar,uint32_t *cfarRstNum1,BBE_CFARRST_T **cfar1Rst,uint32_t *cfarRstNum2,
		BBE_CFARRST_T **cfar2Rst) __attribute__((unused));

// chirp_dir:0-"chirp in row" 1-"chirp in column"
static int udsf_hw_cfar(uint8_t chirp_dir,STRUCT_POINTRST *frameRst,CFAR_CONFIG* range_cfar,CFAR_CONFIG* vel_cfar,uint32_t *cfarRstNum1,BBE_CFARRST_T **cfar1Rst,uint32_t *cfarRstNum2,
		BBE_CFARRST_T **cfar2Rst){

			
	_Bool hitAFlag[CFAR_MAX_NUM];
	_Bool hitBFlag[CFAR_MAX_NUM];
	uint32_t *powAbs=(uint32_t *)paramALG->cfarAbsAddr;
	
	if( vel_cfar != NULL ){
		*cfarRstNum1 = CFAR_HANDLE(vel_cfar);
		*cfar1Rst=(BBE_CFARRST_T *)(BBE_ABUF_BASE+(bbe_regb_str->CFAR_CFG8&0xFFFF));
	}
	if( range_cfar != NULL ){
		*cfarRstNum2 = CFAR_HANDLE(range_cfar);
		*cfar2Rst=(BBE_CFARRST_T *)(BBE_ABUF_BASE+(bbe_regb_str->CFAR_CFG8&0xFFFF));
	}
	uint8_t data_inc = UDFDEF_RANGE_USE;
	if( chirp_dir == 1 ){
		data_inc = UDFDEF_VEL_NUM;
	}
	if( (range_cfar!=NULL) && (vel_cfar!=NULL) ){
		CMPHIT_CFAR_TARGET(*cfarRstNum1,*cfar1Rst,*cfarRstNum2,*cfar2Rst,hitAFlag,hitBFlag);
		udsf_cfarframe(chirp_dir,data_inc,frameRst,*cfar1Rst,*cfarRstNum1,powAbs,hitAFlag,CFAR12_EN);		
	}else if( range_cfar!=NULL ){
		udsf_cfarframe(chirp_dir,data_inc,frameRst,*cfar2Rst,*cfarRstNum2,powAbs,NULL,CFAR2_EN);
	}else if( vel_cfar!=NULL ){
		udsf_cfarframe(chirp_dir,data_inc,frameRst,*cfar1Rst,*cfarRstNum1,powAbs,NULL,CFAR1_EN);
	}else{
		;// none
	}
	
	return 0;
}

#endif
